/**********************************************************************
 Copyright (c) 2020-2023, Unitree Robotics.Co.Ltd. All rights reserved.
***********************************************************************/
#include <iostream>
#include "FSM/State_FixedStand.h"
#include "common/newBeeLeg.h"

/**fixed stand 缺陷:
 * 位置逆解没有考虑轮子半径
 * 换长腿后位置刚度不足
*/

State_FixedStand::State_FixedStand(CtrlComponents *ctrlComp)
                :FSMState(ctrlComp, FSMStateName::FIXEDSTAND, "fixed stand")
{
#ifndef USE_NEWBEE2
    //指定{hip}下目标足端位置
    Vec34 target_feet_end_posB;     //4条腿在{hip}下足端位置
    double a=HIP_LENGTH;
    double b=-a;
    double h=-0.4;      //机身离地高度
    double c=0.1;       //足端x轴偏移
    double d=-c;
    target_feet_end_posB    <<  c,c,d,d,
                                b,a,b,a,
                                h,h,h,h;

    //计算目标关节角
    _targetQ=_ctrlComp->robotModel->getQ(target_feet_end_posB,FrameType::HIP);
#endif
    //计算线性插补步长
    _step_len=(float)1/_duration;
}

void State_FixedStand::enter(){
    //设定电机为位置控制模式
    for(int i=0; i<4; i++){
        if(_ctrlComp->ctrlPlatform == CtrlPlatform::GAZEBO){
#ifdef USE_NEWBEE2
            _lowCmd->setSimStanceGain(i);
#else
            _lowCmd->setNewbeeStanceGain(i);
#endif
        }
        else if(_ctrlComp->ctrlPlatform == CtrlPlatform::REALROBOT){
            _lowCmd->setRealStanceGain(i);
        }
        _lowCmd->setZeroDq(i);
        _lowCmd->setZeroTau(i);
    }
    for(int i=0; i<12; i++){
        _lowCmd->motorCmd[i].q = _lowState->motorState[i].q;    //固定关节角
        _startPos[i] = _lowState->motorState[i].q;              //startPos赋值为当前关节角
    }
    _ctrlComp->setAllStance();
}

void State_FixedStand::run(){
    _percent += _step_len;
    _percent = _percent > 1 ? 1 : _percent;
    for(int j=0; j<12; j++){
        //线性插补, 从当前关节角运动到目标关节角
#ifdef USE_NEWBEE2
        _lowCmd->motorCmd[j].q = (1 - _percent)*_startPos[j] + _percent*_targetPos[j]; 
#else
        _lowCmd->motorCmd[j].q = (1 - _percent)*_startPos[j] + _percent*_targetQ(j);
#endif
    }
}

void State_FixedStand::exit(){
    _percent = 0;
}

FSMStateName State_FixedStand::checkChange(){
    if(_lowState->userCmd == UserCommand::L2_B){
        return FSMStateName::PASSIVE;
    }
    else if(_lowState->userCmd == UserCommand::L2_X){
        return FSMStateName::FREESTAND;
    }
    else if(_lowState->userCmd == UserCommand::START){
        return FSMStateName::TROTTING;
    }
    else if(_lowState->userCmd == UserCommand::L1_X){
        return FSMStateName::BALANCETEST;
    }
    else if(_lowState->userCmd == UserCommand::L1_A){
        return FSMStateName::SWINGTEST;
    }
    else if(_lowState->userCmd == UserCommand::L1_Y){
        return FSMStateName::STEPTEST;
    }
#ifdef COMPILE_WITH_MOVE_BASE
    else if(_lowState->userCmd == UserCommand::L2_Y){
        return FSMStateName::MOVE_BASE;
    }
#endif  // COMPILE_WITH_MOVE_BASE
    else if(_lowState->userCmd==UserCommand::L2_A){
        return FSMStateName::FIXEDSTAND;
    }
    else if(_lowState->userCmd==UserCommand::WHEEL_5){
        return FSMStateName::WHEEL;
    }
}